#include "QuadTree.h"

/*---------------------------------------------------------------------------*/

void QuadTree::getfrustum()
{
GLfloat proj[16];
GLfloat modl[16];
GLfloat clip[16];
GLfloat t;

 glGetFloatv(GL_PROJECTION_MATRIX, proj);
 glGetFloatv(GL_MODELVIEW_MATRIX, modl);

 // Combine the two matrices (multiply projection by modelview)
 clip[ 0] = modl[ 0]*proj[ 0] + modl[ 1]*proj[ 4] + modl[ 2]*proj[ 8] + modl[ 3]*proj[12];
 clip[ 1] = modl[ 0]*proj[ 1] + modl[ 1]*proj[ 5] + modl[ 2]*proj[ 9] + modl[ 3]*proj[13];
 clip[ 2] = modl[ 0]*proj[ 2] + modl[ 1]*proj[ 6] + modl[ 2]*proj[10] + modl[ 3]*proj[14];
 clip[ 3] = modl[ 0]*proj[ 3] + modl[ 1]*proj[ 7] + modl[ 2]*proj[11] + modl[ 3]*proj[15];

 clip[ 4] = modl[ 4]*proj[ 0] + modl[ 5]*proj[ 4] + modl[ 6]*proj[ 8] + modl[ 7]*proj[12];
 clip[ 5] = modl[ 4]*proj[ 1] + modl[ 5]*proj[ 5] + modl[ 6]*proj[ 9] + modl[ 7]*proj[13];
 clip[ 6] = modl[ 4]*proj[ 2] + modl[ 5]*proj[ 6] + modl[ 6]*proj[10] + modl[ 7]*proj[14];
 clip[ 7] = modl[ 4]*proj[ 3] + modl[ 5]*proj[ 7] + modl[ 6]*proj[11] + modl[ 7]*proj[15];

 clip[ 8] = modl[ 8]*proj[ 0] + modl[ 9]*proj[ 4] + modl[10]*proj[ 8] + modl[11]*proj[12];
 clip[ 9] = modl[ 8]*proj[ 1] + modl[ 9]*proj[ 5] + modl[10]*proj[ 9] + modl[11]*proj[13];
 clip[10] = modl[ 8]*proj[ 2] + modl[ 9]*proj[ 6] + modl[10]*proj[10] + modl[11]*proj[14];
 clip[11] = modl[ 8]*proj[ 3] + modl[ 9]*proj[ 7] + modl[10]*proj[11] + modl[11]*proj[15];

 clip[12] = modl[12]*proj[ 0] + modl[13]*proj[ 4] + modl[14]*proj[ 8] + modl[15]*proj[12];
 clip[13] = modl[12]*proj[ 1] + modl[13]*proj[ 5] + modl[14]*proj[ 9] + modl[15]*proj[13];
 clip[14] = modl[12]*proj[ 2] + modl[13]*proj[ 6] + modl[14]*proj[10] + modl[15]*proj[14];
 clip[15] = modl[12]*proj[ 3] + modl[13]*proj[ 7] + modl[14]*proj[11] + modl[15]*proj[15];

 // Extract the numbers for the RIGHT plane
 frustum[0][0] = clip[ 3] - clip[ 0];
 frustum[0][1] = clip[ 7] - clip[ 4];
 frustum[0][2] = clip[11] - clip[ 8];
 frustum[0][3] = clip[15] - clip[12];
 // Normalize the result
 t = 1.0f/(GLfloat)sqrt(frustum[0][0]*frustum[0][0] + frustum[0][1]*frustum[0][1] + frustum[0][2]*frustum[0][2]);
 frustum[0][0] *= t;
 frustum[0][1] *= t;
 frustum[0][2] *= t;
 frustum[0][3] *= t;

 // Extract the numbers for the LEFT plane
 frustum[1][0] = clip[ 3] + clip[ 0];
 frustum[1][1] = clip[ 7] + clip[ 4];
 frustum[1][2] = clip[11] + clip[ 8];
 frustum[1][3] = clip[15] + clip[12];
 // Normalize the result
 t = 1.0f/(GLfloat)sqrt(frustum[1][0]*frustum[1][0] + frustum[1][1]*frustum[1][1] + frustum[1][2]*frustum[1][2]);
 frustum[1][0] *= t;
 frustum[1][1] *= t;
 frustum[1][2] *= t;
 frustum[1][3] *= t;

 // Extract the BOTTOM plane
 frustum[2][0] = clip[ 3] + clip[ 1];
 frustum[2][1] = clip[ 7] + clip[ 5];
 frustum[2][2] = clip[11] + clip[ 9];
 frustum[2][3] = clip[15] + clip[13];
 // Normalize the result
 t = 1.0f/(GLfloat)sqrt(frustum[2][0]*frustum[2][0] + frustum[2][1]*frustum[2][1] + frustum[2][2]*frustum[2][2]);
 frustum[2][0] *= t;
 frustum[2][1] *= t;
 frustum[2][2] *= t;
 frustum[2][3] *= t;

 // Extract the TOP plane
 frustum[3][0] = clip[ 3] - clip[ 1];
 frustum[3][1] = clip[ 7] - clip[ 5];
 frustum[3][2] = clip[11] - clip[ 9];
 frustum[3][3] = clip[15] - clip[13];
 // Normalize the result
 t = 1.0f/(GLfloat)sqrt(frustum[3][0]*frustum[3][0] + frustum[3][1]*frustum[3][1] + frustum[3][2]*frustum[3][2]);
 frustum[3][0] *= t;
 frustum[3][1] *= t;
 frustum[3][2] *= t;
 frustum[3][3] *= t;

 // Extract the FAR plane
 frustum[4][0] = clip[ 3] - clip[ 2];
 frustum[4][1] = clip[ 7] - clip[ 6];
 frustum[4][2] = clip[11] - clip[10];
 frustum[4][3] = clip[15] - clip[14];
 // Normalize the result
 t = 1.0f/(GLfloat)sqrt(frustum[4][0]*frustum[4][0] + frustum[4][1]*frustum[4][1] + frustum[4][2]*frustum[4][2]);
 frustum[4][0] *= t;
 frustum[4][1] *= t;
 frustum[4][2] *= t;
 frustum[4][3] *= t;

 // Extract the NEAR plane
 frustum[5][0] = clip[ 3] + clip[ 2];
 frustum[5][1] = clip[ 7] + clip[ 6];
 frustum[5][2] = clip[11] + clip[10];
 frustum[5][3] = clip[15] + clip[14];
 // Normalize the result
 t = 1.0f/(GLfloat)sqrt(frustum[5][0]*frustum[5][0] + frustum[5][1]*frustum[5][1] + frustum[5][2]*frustum[5][2]);
 frustum[5][0] *= t;
 frustum[5][1] *= t;
 frustum[5][2] *= t;
 frustum[5][3] *= t;
 
}

/*---------------------------------------------------------------------------*/

bool QuadTree::pointinfrustum( Vect3d *point )
{
int i;

 for(i=0; i<6; i++)
  if(frustum[i][0] * point->x + frustum[i][1] * point->y + frustum[i][2] * point->z + frustum[i][3] <= 0)
   return false;
  
return true;
}

/*---------------------------------------------------------------------------*/

bool QuadTree::cubeinfrustum(Sector *sector)
{
Vect3d box_min, box_max;

 box_min.x = sector->box_min.x;
 box_min.y = sector->box_min.y;
 box_min.z = sector->box_min.z;
 box_max.x = sector->box_max.x;
 box_max.y = sector->box_max.y;
 box_max.z = sector->box_max.z;
    
  for (int p=0; p<6; p++)
  {
   if ((frustum[p][0]*box_min.x + frustum[p][1]*box_min.y + frustum[p][2]*box_min.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_max.x + frustum[p][1]*box_min.y + frustum[p][2]*box_min.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_min.x + frustum[p][1]*box_max.y + frustum[p][2]*box_min.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_max.x + frustum[p][1]*box_max.y + frustum[p][2]*box_min.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_min.x + frustum[p][1]*box_min.y + frustum[p][2]*box_max.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_max.x + frustum[p][1]*box_min.y + frustum[p][2]*box_max.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_min.x + frustum[p][1]*box_max.y + frustum[p][2]*box_max.z + frustum[p][3]) > 0.0f)
    continue;
   if ((frustum[p][0]*box_max.x + frustum[p][1]*box_max.y + frustum[p][2]*box_max.z + frustum[p][3]) > 0.0f)
    continue;
   return 0;
  }
 return 1;
}

